#include "motor.hpp"

static void init_Motor()
{
  gpio_deep_sleep_hold_dis();
  for (MotorCHxCfg* mt : motor.mt) {
    gpio_hold_dis((gpio_num_t)mt->pinP);
    gpio_hold_dis((gpio_num_t)mt->pinN);
    pinMode(mt->pinP, OUTPUT);
    pinMode(mt->pinN, OUTPUT);
    digitalWrite(mt->pinP, LOW);
    digitalWrite(mt->pinN, LOW);
  }
   gpio_deep_sleep_hold_dis();

  analogReadResolution(12);
  analogSetAttenuation(ADC_0db);
}

static void loop_Motor(){
  for (MotorCHxCfg* mt : motor.mt)
  {
    motor.ops->hander(mt);
  }
}

static void stop_Motor(){
  for (MotorCHxCfg* mt : motor.mt) {
      digitalWrite(mt->pinP, LOW);
      digitalWrite(mt->pinN, LOW);
      gpio_hold_en((gpio_num_t)mt->pinP);
      gpio_hold_en((gpio_num_t)mt->pinN);
    }
    gpio_deep_sleep_hold_en();
}

void handler_motor(MotorCHxCfg* mt)
{
  if(mt->directionLast != mt->direction)
  {
    if(mt->runtick < 1000/50 && mt->directionLast != DIR_STOP)
    {
      if(mt->runtick == 0)
        Serial.print("delay:");
      else Serial.print(".");
      //防止反冲电压
      mt->runtick++;
      if(mt->directionLast == DIR_LOAD)
      {
        digitalWrite(mt->pinP, LOW);
        digitalWrite(mt->pinN, LOW);
      }else{
        digitalWrite(mt->pinN, LOW);
        digitalWrite(mt->pinP, LOW);
      }
      mt->stuckAdcValue = 0;
      mt->overtick = 0;
    }else
    {
      mt->directionLast = mt->direction;
      switch (mt->direction)
      {
        case DIR_LOAD:
          digitalWrite(mt->pinN, LOW);
          digitalWrite(mt->pinP, HIGH);
          Serial.print("\npinP.");
          Serial.print(mt->pinP);
          Serial.println(" :HIGH");
          Serial.print("pinN.");
          Serial.print(mt->pinN);
          Serial.println(" :LOW");
        break;
        case DIR_UNLOAD:
          digitalWrite(mt->pinP, LOW);
          digitalWrite(mt->pinN, HIGH);
          Serial.print("\npinP.");
          Serial.print(mt->pinP);
          Serial.println(" :LOW");
          Serial.print("pinN.");
          Serial.print(mt->pinN);
          Serial.println(" :HIGH");
        break;
        case DIR_STOP:
          digitalWrite(mt->pinP, LOW);
          digitalWrite(mt->pinN, LOW);
          Serial.print("\npinP.");
          Serial.print(mt->pinP);
          Serial.println(" :LOW");
          Serial.print("pinN.");
          Serial.print(mt->pinN);
          Serial.println(" :LOW");
        default:
          break;
      }
      mt->runtick = 0;
    }
  }else
  {
    mt->stuckAdcValue = analogRead(0);
    if(mt->stuckThreshold > mt->stuckAdcValue)
    {
      //过流保护
      if(mt->overtick < 10)
      {
        mt->overtick++;
      }else
      {
        mt->overtick = 0;
        mt->runtick = 0;
        mt->direction = DIR_OVER;
      }
    }else if(mt->direction == DIR_LOAD)
    {
      if(mt->runtick < 10000/50)
      {
        mt->runtick++;
      }else
      {
        //单次运行10秒超时自动关闭
        mt->runtick = 0;
        mt->direction = DIR_OVER;
      }
    }else if(mt->direction == DIR_UNLOAD)
    {
      if(mt->runtick < 6000/50)
      {
        mt->runtick++;
      }else
      {
        //单次运行6秒超时自动关闭
        mt->direction = DIR_OVER;
      }
    }else{
      // DIR_STOP,DIR_OVER状态不做任何操作
    }
  }
}

static void load(MotorCHxCfg* mt)
{
  // load filament
  mt->direction = DIR_LOAD;
  digitalWrite(mt->pinP, HIGH);
  digitalWrite(mt->pinN, LOW);
}

static void unload(MotorCHxCfg* mt)
{
  // unload filament
  mt->direction = DIR_UNLOAD;
  digitalWrite(mt->pinP, LOW);
  digitalWrite(mt->pinN, HIGH);
}

static void stop(MotorCHxCfg* mt)
{
  mt->direction = DIR_STOP;
  digitalWrite(mt->pinP, LOW);
  digitalWrite(mt->pinN, LOW);
}

static void run_ms(MotorCHxCfg* mt,uint8_t dir,uint32_t ms)
{
  if(dir == DIR_LOAD)
  {
    digitalWrite(mt->pinP, HIGH);
    digitalWrite(mt->pinN, LOW);
  }
  else if(dir == DIR_UNLOAD)
  {
    digitalWrite(mt->pinP, LOW);
    digitalWrite(mt->pinN, HIGH);
  }
  delay(ms);
  digitalWrite(mt->pinP, LOW);
  digitalWrite(mt->pinN, LOW);
}

static void run_ms(MotorCHxCfg* mt,uint32_t ms)
{
  mt->directionLast = mt->direction;
  if(mt->directionLast != DIR_UNLOAD)
    run_ms(mt,DIR_LOAD, ms);
  else if(mt->directionLast != DIR_LOAD) 
    run_ms(mt,DIR_UNLOAD, ms);
}


static void stuck_set(MotorCHxCfg* mt,uint16_t value)
{
  if(value > 4095)
  {
    value = 4095;
  }
  mt->stuckThreshold = value;
}

static boolean stuck_getBool(MotorCHxCfg* mt)
{
  return mt->stuckThreshold > mt->stuckAdcValue;
}

static MotorCHxCfg mt1 = {3, 4, DIR_STOP, DIR_STOP,0, 0, 4096, 0};
static MotorCHxCfg mt2 = {5, 6, DIR_STOP, DIR_STOP,0, 0, 4096, 0};
static MotorCHxCfg mt3 = {7, 8, DIR_STOP, DIR_STOP,0, 0, 4096, 0};
static MotorCHxCfg mt4 = {9,10, DIR_STOP, DIR_STOP,0, 0, 4096, 0};

static MotorCHxOPS ops = {
  .init = &init_Motor,
  .loop = &loop_Motor,
  .keepLow = &stop_Motor,
  .hander = &handler_motor,

  .run_ms = &run_ms,
  .load = &load,
  .unload = &unload,
  .stop = &stop,
  .stuck_value_set = &stuck_set,
  .stuck_state_get = &stuck_getBool
};

MOTOR motor = {
  .mt = {&mt1, &mt2, &mt3, &mt4},
  .ops = &ops,
};
